/**
 * Arduino两轮小车
 * 
 * 本程序可不受限制的用于学习和教学，商业用途请联系作者。
 * 本程序不对用户因使用程序而造成任何形式的损失负责。
 * 
 * site: http://www.xpstem.com/maker/arduino-vehicle
 * author: billy zhang
 */

#include <EEPROM.h>
#include "TwoWheelDriven.h"
#include "AdKeyboard.h"
#include "SoftSerialWrapper.h"
#include "UltrasonicLidar.h"

#include "UltrasonicVehicle.h"
#include "InfraredFollowVehicle.h"
#include "InfraredTrackingVehicle.h"

#include "RemoteControlVehicle.h"
#include "InfraredReceiver.h"
#include "PS2xReceiver.h"
#include "BluetoothReceiver.h"

#define IR_Recv_In 2
#define Servo_Out 3  // PWM

#define Soft_Serial_Rx 2  //复用
#define Soft_Serial_Tx 3  //复用

#define Left_Motor_Dir 4  
#define Left_Motor_En 5  //PWM
#define Right_Motor_Dir 7  
#define Right_Motor_En 6  //PWM

#define Ultra_Echo 8
#define Ultra_Trig 9

#define PS2_CMD 11 //SPI
#define PS2_CLK 12 //SPI
#define PS2_DAT 13 //SPI
#define PS2_CS 10

#define TWI_SCL A0 //TWI
#define TWI_SDA A1 //TWI
#define Left_Tracking_In A2
#define Right_Tracking_In A3   
#define Ad_Keyboard_In A4
#define Mode_In A5

const int Soft_Serial_Bound = 9600;
const int Led_Controller_Twi_Addr = 0x27;

AdKeyboard keyboard(Ad_Keyboard_In);
TwoWheelDriven driven(Left_Motor_Dir, Left_Motor_En, Right_Motor_Dir, Right_Motor_En);

TwiBus twiBus(TWI_SCL, TWI_SDA);
LedController ledCtrl(twiBus, Led_Controller_Twi_Addr);
XPVehicle *vehicle;

int g_mode = -1;
int g_subMode = -1;
int g_addr = 0;

void(* resetFunc)(void) = 0;

void setup() {

  Serial.begin(9600);
  pinMode(Mode_In, INPUT);

  g_mode = EEPROM.read(g_addr);
  g_subMode = EEPROM.read(g_addr+1);
  vehicle = createVehicle();
}

void loop() {

  bool modeChanged = false;
  uint8_t mode = digitalRead(Mode_In);
  uint8_t subMode = 0;
  if (g_mode != mode)
  {
    modeChanged = true;
  }
  else
  {
    subMode = keyboard.value();
    if (g_subMode != subMode)
    {
      modeChanged = true;
    }
  }

  if (modeChanged) 
  {
    EEPROM.write(g_addr, mode);
    EEPROM.write(g_addr, g_subMode);
    resetFunc();
  }
  
  vehicle->drive();
  
}

XPVehicle* createVehicle()
{
  if (g_mode == 0) 
  {
    // 程控模式
    switch (g_subMode)
    {
     case 2:
       UltrasonicLidar *lidar2 = new UltrasonicLidar(Ultra_Echo, Ultra_Trig, Servo_Out);
       vehicle = new InfraredFollowVehicle(driven, ledCtrl, *lidar2, IR_Recv_In);
       break;

     case 3:
      vehicle = new InfraredTrackingVehicle(driven, ledCtrl, Left_Tracking_In, Right_Tracking_In);
      break;

     default:
      UltrasonicLidar *lidar1 = new UltrasonicLidar(Ultra_Echo, Ultra_Trig, Servo_Out);
      vehicle = new UltrasonicVehicle(driven, ledCtrl, *lidar1);
      break;
     
    }
  }
  else
  {
    switch (g_subMode)
    {
     case 2:
       PS2xReceiver *ps2xReceiver = new PS2xReceiver(PS2_CLK, PS2_CMD, PS2_DAT, PS2_CS); 
       vehicle = new RemoteControlVehicle(driven, ledCtrl, *ps2xReceiver);
       break;
       
     case 3:
      SoftSerialWrapper *bleSerial = new SoftSerialWrapper(Soft_Serial_Rx, Soft_Serial_Tx, Soft_Serial_Bound);
       BluetoothReceiver *bleReceiver = new BluetoothReceiver(*bleSerial);
       vehicle = new RemoteControlVehicle(driven, ledCtrl, *bleReceiver);
       break;
       
     default:
       InfraredReceiver *irReceiver = new InfraredReceiver(IR_Recv_In);
       vehicle = new RemoteControlVehicle(driven, ledCtrl, *irReceiver);
       break;
    }
  }
  
  vehicle->setup();
}
